/*
 * BiConjugateGradientStabilizedSolver.cs
 * 
 * Copyright (c) 2003-2004, dnAnalytics Project. All rights reserved.
*/

using System;
using dnAnalytics.Exceptions;
using dnAnalytics.LinearAlgebra.Solvers.Preconditioners;
using dnAnalytics.Resources;

namespace dnAnalytics.LinearAlgebra.Solvers
{
    /// <summary>
    /// A Bi-Conjugate Gradient stabilized iterative matrix solver.
    /// </summary>
    /// <remarks>
    /// <para>
    /// The Bi-Conjugate Gradient Stabilized (BiCGStab) solver is an 'improvement'
    /// of the standard Conjugate Gradient (CG) solver. Unlike the CG solver the
    /// BiCGStab can be used on non-symmetric matrices. <br/>
    /// Note that much of the success of the solver depends on the selection of the
    /// proper preconditioner.
    /// </para>
    /// <para>
    /// The Bi-CGSTAB algorithm was taken from: <br/>
    /// Templates for the solution of linear systems: Building blocks
    /// for iterative methods
    /// <br/>
    /// Richard Barrett, Michael Berry, Tony F. Chan, James Demmel,
    /// June M. Donato, Jack Dongarra, Victor Eijkhout, Roldan Pozo,
    /// Charles Romine and Henk van der Vorst
    /// <br/>
    /// Url: <a href="http://www.netlib.org/templates/Templates.html">http://www.netlib.org/templates/Templates.html</a>
    /// <br/>
    /// Algorithm is described in Chapter 2, section 2.3.8, page 27
    /// </para>
    /// <para>
    /// The example code below provides an indication of the possible use of the
    /// solver.
    /// </para>
    /// </remarks>
    /// <include file='../../../../examples/trunk/LinearAlgebra/Solvers/BicgstabSolver.xml' path='example'/> 
    public sealed class BiConjugateGradientStabilizedSolver : IIterativeMatrixSolver
    {
        private Matrix mCoefficients;
        private IPreconditioner mPreconditioner;
        private ConvergenceMonitor mConvergenceMonitor;

        /// <summary>
        /// Initializes a new instance of the <b>BiConjugateGradientStabilizedSolver</b> class.
        /// </summary>
        /// <remarks>
        /// When using this constructor the solver will use the <see cref="ConvergenceMonitor"/> with
        /// the standard settings and a <see cref="UnitPreconditioner"/> as preconditioner.
        /// </remarks>
        /// <param name="coefficientMatrix">The coefficient matrix.</param>
        /// <exception cref="ArgumentNullException">If the <paramref name="coefficientMatrix"/> is <b>null</b>.</exception>
        /// <exception cref="NotSquareMatrixException">If the <paramref name="coefficientMatrix"/> is not square.</exception>
        public BiConjugateGradientStabilizedSolver(Matrix coefficientMatrix) : 
            this(coefficientMatrix, null, null)
        {}

        /// <summary>
        /// Initializes a new instance of the <b>BiConjugateGradientStabilizedSolver</b> class.
        /// </summary>
        /// <remarks>
        /// <para>
        /// When using this constructor the solver will use a <see cref="UnitPreconditioner"/> 
        /// as preconditioner.
        /// </para>
        /// <para>
        /// The main advantages of using a user defined <see cref="ConvergenceMonitor"/> are:
        /// <list type="number">
        /// <item>It is possible to set the desired convergence limits.</item>
        /// <item>
        /// It is possible to check the reason for which the solver finished 
        /// the iterative procedure by  calling the 
        /// <see cref="Solvers.ConvergenceMonitor.SolutionStatus"/> property.
        /// </item>
        /// </list>
        /// </para>
        /// </remarks>
        /// <param name="coefficientMatrix">The coefficient matrix.</param>
        /// <param name="monitor">The <see cref="ConvergenceMonitor"/> that will be used to
        /// monitor the iterative process.</param>
        /// <exception cref="ArgumentNullException">If the <paramref name="coefficientMatrix"/> is <b>null</b>.</exception>
        /// <exception cref="NotSquareMatrixException">If the <paramref name="coefficientMatrix"/> is not square.</exception>
        public BiConjugateGradientStabilizedSolver(Matrix coefficientMatrix, ConvergenceMonitor monitor)
            : this(coefficientMatrix, null, monitor)
        {}

        /// <summary>
        /// Initializes a new instance of the <b>BiConjugateGradientStabilizedSolver</b> class.
        /// </summary>
        /// <remarks>
        /// When using this constructor the solver will use the <see cref="ConvergenceMonitor"/> with
        /// the standard settings.
        /// </remarks>
        /// <param name="coefficientMatrix">The coefficient matrix.</param>
        /// <param name="preconditioner">The <see cref="IPreconditioner"/> that will be used to precondition the
        /// matrix equation.</param>
        /// <exception cref="ArgumentNullException">If the <paramref name="coefficientMatrix"/> is <b>null</b>.</exception>
        /// <exception cref="NotSquareMatrixException">If the <paramref name="coefficientMatrix"/> is not square.</exception>
        public BiConjugateGradientStabilizedSolver(Matrix coefficientMatrix, IPreconditioner preconditioner)
            : this(coefficientMatrix, preconditioner, null)
        {}

        /// <summary>
        /// Initializes a new instance of the <b>BiConjugateGradientStabilizedSolver</b> class.
        /// </summary>
        /// <remarks>
        /// <para>
        /// The main advantages of using a user defined <see cref="ConvergenceMonitor"/> are:
        /// <list type="number">
        /// <item>It is possible to set the desired convergence limits.</item>
        /// <item>
        /// It is possible to check the reason for which the solver finished 
        /// the iterative procedure by  calling the 
        /// <see cref="Solvers.ConvergenceMonitor.SolutionStatus"/> property.
        /// </item>
        /// </list>
        /// </para>
        /// </remarks>
        /// <param name="coefficientMatrix">The coefficient matrix.</param>
        /// <param name="preconditioner">The <see cref="IPreconditioner"/> that will be used to precondition the
        /// matrix equation.</param>
        /// <param name="monitor">The <see cref="ConvergenceMonitor"/> that will be used to
        /// monitor the iterative process.</param>
        /// <exception cref="ArgumentNullException">If the <paramref name="coefficientMatrix"/> is <b>null</b>.</exception>
        /// <exception cref="NotSquareMatrixException">If the <paramref name="coefficientMatrix"/> is not square.</exception>
        /// <include file='../../../../examples/trunk/LinearAlgebra/Solvers/BicgstabSolverConstructor.xml' path='example'/> 
        public BiConjugateGradientStabilizedSolver(Matrix coefficientMatrix, IPreconditioner preconditioner,
                                                   ConvergenceMonitor monitor)
        {
            if (coefficientMatrix == null)
            {
                throw new ArgumentNullException("coefficientMatrix", Strings.NullParameterException);
            }

            if (coefficientMatrix.Rows != coefficientMatrix.Columns)
            {
                throw new NotSquareMatrixException();
            }

            mCoefficients = coefficientMatrix;
            mConvergenceMonitor = monitor;
            mPreconditioner = preconditioner;
        }

        /// <summary>
        /// Returns the <see cref="Matrix"/> on which the solver works.
        /// </summary>
        internal Matrix Matrix
        {
            get
            {
                return mCoefficients;
            }
        }

        /// <summary>
        /// Returns the <see cref="IPreconditioner"/> that is used by the solver
        /// to accelerate the iteration process.
        /// </summary>
        internal IPreconditioner Preconditioner
        {
            get
            {
                return mPreconditioner;
            }
        }

        /// <summary>
        /// Returns the <see cref="ConvergenceMonitor"/> that is used by the solver
        /// to check the iterative process.
        /// </summary>
        internal ConvergenceMonitor ConvergenceMonitor
        {
            get
            {
                return mConvergenceMonitor;
            }
        }

        /// <summary>
        /// Solves the standard matrix equation <b>Ax = b</b>.
        /// </summary>
        /// <param name="vector">The right hand side vector, <b>b</b>.</param>
        /// <returns>
        /// The left hand side vector, <b>x</b>.
        /// </returns>
        /// <remarks>
        /// When using the <b>Solve</b> method consider carefully if the
        /// left hand side vector can be reused. If so it would be better to
        /// use the overloaded <b>Solve</b> method.
        /// </remarks>
        /// <exception cref="ArgumentNullException">If <paramref name="vector"/> is <b>null</b>.</exception>
        /// <include file='../../../../examples/trunk/LinearAlgebra/Solvers/BicgstabSolverSolve.xml' path='example'/> 
        public Vector Solve(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException(Strings.NullParameterException);
            }
            Vector result = VectorBuilder.CreateVector(mCoefficients.Rows, VectorType.Dense);
            Solve(vector, result);
            return result;
        }

        /// <summary>
        /// Solves the standard matrix equation <b>Ax = b</b>.
        /// </summary>
        /// <param name="vector">The left hand side vector, <b>x</b>.</param>
        /// <param name="result">The right hand side vector, <b>b</b>.</param>
        /// <exception cref="ArgumentNullException">If <paramref name="vector"/> is <b>null</b>.</exception>
        /// <exception cref="ArgumentNullException">If <paramref name="result"/> is <b>null</b>.</exception>
        /// <exception cref="NotConformableException">If the size of <paramref name="vector"/> and 
        /// <paramref name="result"/> differ.</exception>
        /// <include file='../../../../examples/trunk/LinearAlgebra/Solvers/BicgstabSolverSolve.xml' path='example'/> 
        public void Solve(Vector vector, Vector result)
        {
            if (vector == null)
            {
                throw new ArgumentNullException(Strings.NullParameterException);
            }
            if (result == null)
            {
                throw new ArgumentNullException(Strings.NullParameterException);
            }
            if (result.Count != vector.Count)
            {
                throw new NotConformableException(Strings.ParametersNotConformable);
            }
            // Set the convergence monitor
            if (mConvergenceMonitor == null)
            {
                mConvergenceMonitor = new ConvergenceMonitor(mCoefficients.InfinityNorm());
            }
            mConvergenceMonitor.VectorNorm(vector.InfinityNorm());

            if (mPreconditioner == null)
            {
                mPreconditioner = new UnitPreconditioner(mCoefficients);
            }

            // Compute r_0 = b - Ax_0 for some initial guess x_0
            // In this case we take x_0 = vector
            // This is basically a SAXPY so it could be made a lot faster
            Vector residuals = VectorBuilder.CreateVector(mCoefficients.Rows, VectorType.Dense);
            CalculateTrueResidual(residuals,result, vector);
            
            //Choose r~ (for example, r~ = r_0)
            Vector tempResiduals = residuals.Clone();

            // create five temporary vectors needed to hold temporary
            // coefficients. All vectors are mangled in each iteration.
            // These are defined here to prevent stressing the garbage collector
            Vector p = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);
            Vector pDash = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);
            Vector nu = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);
            Vector s = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);
            Vector sDash = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);
            Vector t = VectorBuilder.CreateVector(residuals.Count, VectorType.Dense);

            // create some temporary double variables that are needed
            // to hold values in between iterations
            double currentRho = 0;
            double alpha = 0;
            double omega = 0;

            int iterationNumber = 0;
            while (mConvergenceMonitor.ShouldCalculationContinue(iterationNumber, result, residuals))
            {
                // rho_(i-1) = r~^T r_(i-1) // dotproduct r~ and r_(i-1)
                double oldRho = currentRho;
                currentRho = tempResiduals.DotProduct(residuals);

                // if (rho_(i-1) == 0) // METHOD FAILS
                if (System.Math.Abs(currentRho) < double.Epsilon)
                {
                    throw new BiConjugateGradientBrakedownException(Strings.RhoTypeBreakdown);
                }

                if (iterationNumber == 0)
                {
                    // p_i = r_(i-1)
                    residuals.CopyTo(p);
                }
                else
                {
                    // beta_(i-1) = (rho_(i-1)/rho_(i-2))(alpha_(i-1)/omega(i-1))
                    double beta = (currentRho/oldRho)*(alpha/omega);
                    // p_i = r_(i-1) + beta_(i-1)(p_(i-1) - omega_(i-1) * nu_(i-1))
                    nu.Multiply(omega);
                    p.Subtract(nu);
                    p.Multiply(beta);
                    p.Add(residuals);
                }

                // SOLVE Mp~ = p_i // M = preconditioner
                mPreconditioner.Solve(p, pDash);
                // nu_i = Ap~
                mCoefficients.Multiply(pDash, nu);

                // alpha_i = rho_(i-1)/ (r~^T nu_i) = rho / dotproduct(r~ and nu_i)
                alpha = currentRho*1/tempResiduals.DotProduct(nu);

                // s = r_(i-1) - alpha_i nu_i
                nu.CopyTo(s);
                s.Multiply(-alpha);
                s.Add(residuals);

                // Check if we're converged. If so then stop. Otherwise continue;
                if (!mConvergenceMonitor.ShouldCalculationContinue(iterationNumber,result,s))
                {
                    pDash.Multiply(alpha);
                    result.Add(sDash);
                    result.Add(pDash);
                    
                    // Calculate the true residual
                    CalculateTrueResidual(residuals, result, vector);
                    
                    // Now recheck the convergence
                    if (!mConvergenceMonitor.ShouldCalculationContinue(iterationNumber, result, residuals))
                    {
                        // We're all good now.
                        return;
                    }
                    else
                    {
                        // Continue the calculation
                        iterationNumber++;
                        continue;
                    }
                }

                // SOLVE Ms~ = s
                mPreconditioner.Solve(s, sDash);

                // t = As~
                mCoefficients.Multiply(sDash, t);

                // omega_i = t^T s / t^T t
                omega = t.DotProduct(s)/t.DotProduct();

                // x_i = x_(i-1) + alpha_i p^ + omega_i s^
                sDash.Multiply(omega);
                pDash.Multiply(alpha);
                result.Add(sDash);
                result.Add(pDash);

                t.Multiply(-omega);
                s.CopyTo(residuals);

                residuals.Add(t);

                // for continuation it is necessary that omega_i != 0.0
                if (System.Math.Abs(omega) < double.Epsilon)
                {
                    throw new BiConjugateGradientBrakedownException(Strings.OmegaTypeBreakdown);
                }
                
                if (!mConvergenceMonitor.ShouldCalculationContinue(iterationNumber, result, residuals))
                {
                    // Recalculate the residuals and go round again. This is done to ensure that
                    // we have the proper residuals.
                    // The residual calculation based on omega_i * s can be off by a factor 10. So here
                    // we calculate the real residual (which can be expensive) but we only do it if we're
                    // sufficiently close to the finish.
                    CalculateTrueResidual(residuals, result, vector);
                }
                               
                iterationNumber++;
            }
        }

        // Calculates the true residual of the matrix equation Ax = b accoording to:
        // residual = b - Ax
        private void CalculateTrueResidual(Vector residual, Vector x, Vector b)
        {
            // -Ax = residual
            mCoefficients.Multiply(x, residual);
            residual.Negate();
                    
            // residual + b
            residual.Add(b);
        }
    }
}
